#include "arduino_serial.h"
#include "mysockets.h"
#include "time.h"
#include <stdio.h>
#include <pthread.h>

int arduino_fd = 0;

void *f_arduino_reader(){
	char msg[255];
	int r;

	while(1){
		//continuously reads and displays messages from the arduino
		r = serialport_read_until(arduino_fd, msg, '\n'); 
		if(r != -1){
			fprintf(stderr,"Arduino msg: '%s'\n",msg);
		}
	}
}

//reads n bytes from fd into msg
//returns 0 on success, -1 on error
int read_n_bytes(int n, int fd, char *msg){

	int num_read;
	int r;

	num_read = 0;

	while(num_read < n){
		r=read(fd, msg+num_read, n);
		if(r<0){
			return -1;
		}
		num_read+=r;
	}
	msg[n] = '\0';

	return 0;

}

int main(int argc, char ** argv){

	char * hostname;	//holds the host's name
	int pwm_13_14, pwm_15_16, x_camera, y_camera;
	char header_byte;
	int r;
	char c;
	int n;
	char msg[256]; //max msg length is pre-determined
	int tty_num;
	pthread_t pt_arduino_reader;
	char str_arduino_port[256];

	//check for correct usage
	if(argc != 2){
		fprintf(stderr, "Usage: ./myserver_input_test arduino-ttyACM#\n");
		exit(1);
	}

	//the arduino shows up either on ttyACM0 or ttyACM1
	tty_num = atoi(argv[1]);
	sprintf(str_arduino_port,"/dev/ttyACM%d",tty_num);

	//get a file descriptor to communicate with the arduino
	arduino_fd = serialport_init(str_arduino_port, 9600);
	if(arduino_fd==-1){
		fprintf(stderr,"Error: could not open arduino's serial port\n");	
		return -1;
	}

	//create a thread that just reads lines from the arduino
	/*
	r = pthread_create(&pt_arduino_reader,NULL,f_arduino_reader,NULL);
	if(r == -1){
		fprintf(stderr,"Error: pthread_create\n");
		return -1;
	}
	*/

	while(1){

		fprintf(stderr,"Input 'pwm_13_14 pwm_15_16'\n");
		r = fscanf(stdin,"%d %d",&pwm_13_14,&pwm_15_16);
		if(r != 2 || pwm_13_14 < 0 || pwm_13_14 > 255 || pwm_15_16 < 0 || pwm_15_16 > 255){
			continue;
		}

		//send header byte, then the message:

		//pwm_13_14
		header_byte = (char)1;
		do{
			r = serialport_writebyte(arduino_fd, header_byte);
		}while(r==-1);

		//pwm_13_14 = (pwm_13_14/(255/40));
		do{
			r = serialport_writebyte(arduino_fd, (char)pwm_13_14);
		}while(r==-1);

		//pwm_15_16
		header_byte = (char)2;
		do{
			r = serialport_writebyte(arduino_fd, header_byte);
		}while(r==-1);

		//pwm_15_16 = (pwm_15_16/(255/40) + 40);
		do{
			r = serialport_writebyte(arduino_fd, (char)pwm_15_16);
		}while(r==-1);

		fprintf(stderr,"Wrote to arduino:\n"
				"   pwm_13_14: %d\n"
				"   pwm_15_16: %d\n"
				,pwm_13_14
				,pwm_15_16);

	}
}
